/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, 2013, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * Author: Eitan Marder-Eppstein
 *         David V. Lu!!
 *********************************************************************/
#include <costmap_2d/visual_layer.h>
#include <costmap_2d/costmap_math.h>
#include <pluginlib/class_list_macros.h>

PLUGINLIB_EXPORT_CLASS(costmap_2d::VisualLayer, costmap_2d::Layer)

using costmap_2d::NO_INFORMATION;
using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::FREE_SPACE;

using costmap_2d::ObservationBuffer;
using costmap_2d::Observation;


using namespace std;
using namespace cv;
namespace costmap_2d
{

void VisualLayer::onInitialize()
{
  ros::NodeHandle nh("~/" + name_), g_nh;
  rolling_window_ = layered_costmap_->isRolling();

  bool track_unknown_space;
  bool clearing, marking;
  nh.param("track_unknown_space", track_unknown_space, layered_costmap_->isTrackingUnknown());
  if (!track_unknown_space)
    default_value_ = NO_INFORMATION;
  else
    default_value_ = FREE_SPACE;

  VisualLayer::matchSize();
  current_ = true;

  global_frame_ = layered_costmap_->getGlobalFrameID();
  ROS_INFO("visual layer global_frame_:%s",global_frame_.c_str());
  double transform_tolerance;
  nh.param("transform_tolerance", transform_tolerance, 0.2);

  std::string topics_string;
  // get the topics that we'll subscribe to from the parameter server
  nh.param("visual_sources", topics_string, std::string(""));
  ROS_INFO("    Subscribed to Topics: %s", topics_string.c_str());

  double observation_keep_time = 0.0, expected_update_rate, min_visual_height, max_visual_height;
  nh.param("min_visual_height", min_visual_height, -0.6);
  nh.param("max_visual_height", max_visual_height, 2.0);
  // get our tf prefix
  ros::NodeHandle prefix_nh;
  const std::string tf_prefix = tf::getPrefixParam(prefix_nh);

  // now we need to split the topics based on whitespace which we can use a stringstream for
  std::stringstream ss(topics_string);

  std::string source;
  while (ss >> source)
  {
    ros::NodeHandle source_node(nh, source);

    std::string topic, sensor_frame;
    double raytrace_range = 3.0;
    double visual_range = 2.5;
    source_node.param("topic", topic, source);
    source_node.param("sensor_frame", sensor_frame, std::string(""));
    source_node.param("expected_update_rate", expected_update_rate, 0.0);
    source_node.param("clearing", clearing, false);
    source_node.param("marking", marking, true);

    if (!sensor_frame.empty())
    {
      sensor_frame = tf::resolve(tf_prefix, sensor_frame);
    }

    ROS_INFO("visual topic:%s",topic.c_str());
    // create an observation buffer
    observation_buffers_.push_back(
        boost::shared_ptr < ObservationBuffer
            > (new ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_visual_height,
                                      max_visual_height, visual_range, raytrace_range, *tf_, global_frame_,
                                      sensor_frame, transform_tolerance)));

    // check if we'll add this buffer to our marking observation buffers
    if (marking)
      marking_buffers_.push_back(observation_buffers_.back());

    // check if we'll also add this buffer to our clearing observation buffers
    if (clearing)
      clearing_buffers_.push_back(observation_buffers_.back());

    boost::shared_ptr < message_filters::Subscriber<sensor_msgs::PointCloud2>
            > sub(new message_filters::Subscriber<sensor_msgs::PointCloud2>(g_nh, topic, 10));

    boost::shared_ptr < tf::MessageFilter<sensor_msgs::PointCloud2>
        > filter(new tf::MessageFilter<sensor_msgs::PointCloud2>(*sub, *tf_, global_frame_, 10));


    filter->registerCallback(
        boost::bind(&VisualLayer::OnMapReceived, this, _1, observation_buffers_.back()));

    observation_subscribers_.push_back(sub);
    observation_notifiers_.push_back(filter);

    //observation_notifiers_.back()->setTolerance(ros::Duration(0.05));
  }
  dsrv_ = NULL;
  setupDynamicReconfigure(nh);
}
void VisualLayer::setupDynamicReconfigure(ros::NodeHandle& nh)
{
  dsrv_ = new dynamic_reconfigure::Server<costmap_2d::VisualPluginConfig>(nh);
  dynamic_reconfigure::Server<costmap_2d::VisualPluginConfig>::CallbackType cb = boost::bind(
      &VisualLayer::reconfigureCB, this, _1, _2);
  dsrv_->setCallback(cb);
}

void VisualLayer::reconfigureCB(costmap_2d::VisualPluginConfig &config, uint32_t level)
{
  enabled_ = config.enabled;
  footprint_clearing_enabled_ = config.footprint_clearing_enabled;
  max_visual_height_ = config.max_visual_height_;
  min_visual_height_ = config.min_visual_height_;
  combination_method_ = config.combination_method;
}
VisualLayer::~VisualLayer()
{
    if (dsrv_)
        delete dsrv_;
}
void VisualLayer::laserScanCallback(const sensor_msgs::LaserScanConstPtr& message,const boost::shared_ptr<ObservationBuffer>& buffer) 
{
    //ROS_INFO("test laser ... ... ...");
    // buffer the point cloud
    
}
void VisualLayer::OnMapReceived(const sensor_msgs::PointCloud2ConstPtr& message,const boost::shared_ptr<ObservationBuffer>& buffer) 
{
    //ROS_INFO("test camera ... ... ...");
    // buffer the point cloud
    buffer->lock();
    buffer->bufferCloud(*message);
    buffer->unlock();
}

void VisualLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
                                          double* min_y, double* max_x, double* max_y)
{
  int size_x_ = getSizeInCellsX();
  int size_y_ = getSizeInCellsY();
  int threshold = 1.2 / getResolution();
  int x0 = size_x_ / 2 - threshold ;
  int y0 = size_y_ / 2 - threshold;
  int xn = size_x_ / 2 + threshold;
  int yn = size_y_ / 2 + threshold;
  unsigned int x0_ = x0 > 0? x0 : 0;
  unsigned int y0_ = y0 > 0? y0 : 0;
  unsigned int xn_ = xn < size_x_? xn : size_x_;
  unsigned int yn_ = yn < size_y_? yn : size_y_; 
  
  originProtectResetMap(x0_,y0_,xn_,yn_);

  if (rolling_window_)
    updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2 ,false);
  if (!enabled_)
    return;
  useExtraBounds(min_x, min_y, max_x, max_y);
  bool current = true;
  std::vector<Observation> observations;

  // get the marking observations
  current = current && getMarkingObservations(observations);

  // update the global current status
  current_ = current;

  for (std::vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it)
  {
    const Observation& obs = *it;

    const pcl::PointCloud<pcl::PointXYZ>& cloud = *(obs.cloud_);

    double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_;
    //ROS_INFO("...................................................................cloud size:%d",cloud.size());
    for (unsigned int i = 0; i < cloud.points.size(); ++i)
    {
      double px = cloud.points[i].x, py = cloud.points[i].y, pz = cloud.points[i].z;
      // now we need to compute the map coordinates for the observation
      //if(pz>0 && pz<0.1)
      //  ROS_INFO("px:%f,py:%f",px,py);
      unsigned int mx, my;
      if (!worldToMap(px, py, mx, my))
      {
        ROS_DEBUG("Computing map coords failed");
        continue;
      }

      // place the new obstacles
      //if(pz>0 && pz<0.1)
      //  ROS_INFO("px:%d,py:%d",mx,my);
      unsigned int index = getIndex(mx, my);
      costmap_[index] = LETHAL_OBSTACLE;
      touch(px, py, min_x, min_y, max_x, max_y);
   
    }
  }
  updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
}
void VisualLayer::updateFootprint(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
                                    double* max_x, double* max_y)
{
    if (!footprint_clearing_enabled_) return;
    transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(), transformed_footprint_);

    for (unsigned int i = 0; i < transformed_footprint_.size(); i++)
    {
      touch(transformed_footprint_[i].x, transformed_footprint_[i].y, min_x, min_y, max_x, max_y);
    }
}

bool VisualLayer::getMarkingObservations(std::vector<Observation>& marking_observations) const
{
  bool current = true;
  // get the marking observations
  for (unsigned int i = 0; i < marking_buffers_.size(); ++i)
  {
    marking_buffers_[i]->lock();
    marking_buffers_[i]->getObservations(marking_observations);
    current = marking_buffers_[i]->isCurrent() && current;
    marking_buffers_[i]->unlock();
  }
  marking_observations.insert(marking_observations.end(),
                              static_marking_observations_.begin(), static_marking_observations_.end());
  return current;
}
void VisualLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
  if (!enabled_)
    return;

  if (footprint_clearing_enabled_)
  {
    setConvexPolygonCost(transformed_footprint_, costmap_2d::FREE_SPACE);
  }
  //ROS_INFO("combination_method_:%d",combination_method_);
  int i = 1;
  switch (i)//combination_method_)
  {
    case 0:  // Overwrite
      updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j);
      break;
    case 1:  // Maximum
      updateWithMax(master_grid, min_i, min_j, max_i, max_j);
      break;
    default:  // Nothing
      break;
  }
  //printf("visual charMap address:%p \n",master_grid.getCharMap());
}

void VisualLayer::activate()
{
  // if we're stopped we need to re-subscribe to topics
  for (unsigned int i = 0; i < observation_subscribers_.size(); ++i)
  {
    if (observation_subscribers_[i] != NULL)
      observation_subscribers_[i]->subscribe();
  }

  for (unsigned int i = 0; i < observation_buffers_.size(); ++i)
  {
    if (observation_buffers_[i])
      observation_buffers_[i]->resetLastUpdated();
  }
}

void VisualLayer::deactivate()
{
  for (unsigned int i = 0; i < observation_subscribers_.size(); ++i)
  {
    if (observation_subscribers_[i] != NULL)
      observation_subscribers_[i]->unsubscribe();
  }
}

void VisualLayer::reset()
{
    deactivate();
    resetMaps();
    current_ = true;
    activate();
}

}  // namespace costmap_2d
